#include "KDTree.h"
#include "GeometryUtils.h"
#include <vector>
#include <set>
#include <cmath>

namespace 
	{
	const size_t MAX_PRIMITIVES_PER_NODE = 25;
	const double MAX_BOX = std::numeric_limits<double>::max(); 

	size_t recursion_depth = 0;
	size_t max_recursion_depth = 0;
	}
KDTree::Node::Node() : m_triangles_in_subtree(0)
	{
	TVector3 center1 = TVector3(0, 0, 0);
	TVector3 u0 = TVector3(1, 0, 0);
  TVector3 u1 = TVector3(0, 1, 0);
  TVector3 u2 = TVector3(0, 0, 1);
	double ext1 = fabs(MAX_BOX);
	double ext2 = fabs(MAX_BOX);
	double ext3 = fabs(MAX_BOX);
  TBox3 box = TBox3(center1, u0, u1, u2, ext1, ext2, ext3);
	m_bounding_box = box;
	}

KDTree::Node::Node(TBox3& i_box):mp_left(nullptr), mp_right(nullptr), mp_middle(nullptr), m_triangles_in_subtree(0)
	{
	m_bounding_box = i_box;
	}

KDTree::KDTree(Model3D::Triangle3DIteratorConst begin, Model3D::Triangle3DIteratorConst end)
	{
	Model3D::Triangle3DIteratorConst runner = begin;
	std::vector<const Triangle3D*> triangles;
	while(runner != end)
		{
		triangles.push_back(&(*runner));
		++runner;
		}
	mp_root.reset(new KDTree::Node());	
	_CreateTree(triangles, mp_root);

	//m_number_of_triangles = _HowManyTriangles(mp_root);  call a method for counting number of triangles in the tree 
	m_max_recursion_depth = max_recursion_depth;
	}

size_t KDTree::_HowManyTriangles(std::shared_ptr<KDTree::Node> i_node)
	{
	if (!i_node)
		{
		return 0;
		}
	else if (i_node->m_triangles.size() > 0)
		{
		return i_node->m_triangles.size();
		}
	else 
		{
		return _HowManyTriangles(i_node->mp_left) + _HowManyTriangles(i_node->mp_middle) + _HowManyTriangles(i_node->mp_right);
		}
	}


std::pair<double, double> KDTree::_FindMaxMinCoordinate (const Triangle3D* i_triangle, const size_t i_dimention)
	{
	double first; double second; double third;
	
	first = i_triangle->GetPoint(0)[i_dimention];
	second = i_triangle->GetPoint(1)[i_dimention];
	third =  i_triangle->GetPoint(2)[i_dimention];

	double minPoint = std::min(first, std::min(second, third));
	double maxPoint = std::max(first, std::max(second, third));
	return std::make_pair(minPoint, maxPoint);

	}

void KDTree::_FitTBoxForTriangles(TBox3& io_bounding_box, const std::vector<const Triangle3D*>& i_triangles)
	{
	double max_x = 0, min_x = MAX_BOX, min_y = MAX_BOX, max_y = 0, max_z = 0, min_z = MAX_BOX;


	for(size_t i = 0; i < i_triangles.size(); ++i)
		{
		max_x = std::max(max_x, _FindMaxMinCoordinate(i_triangles[i], 0).second);
		min_x = std::min(min_x, _FindMaxMinCoordinate(i_triangles[i], 0).first);
		max_y = std::max(max_y, _FindMaxMinCoordinate(i_triangles[i], 1).second);
		min_y = std::min(min_y, _FindMaxMinCoordinate(i_triangles[i], 1).first); 
		max_z = std::max(max_z, _FindMaxMinCoordinate(i_triangles[i], 2).second);
		min_z = std::min(min_z, _FindMaxMinCoordinate(i_triangles[i], 2).first);
		}
	max_x = std::min(max_x, io_bounding_box.Center[0] + io_bounding_box.Extent[0]);
	max_y = std::min(max_y, io_bounding_box.Center[1] + io_bounding_box.Extent[1]);
	max_z = std::min(max_z, io_bounding_box.Center[2] + io_bounding_box.Extent[2]);

	min_x = std::max(min_x, io_bounding_box.Center[0] - io_bounding_box.Extent[0]);
	min_y = std::max(min_y, io_bounding_box.Center[1] - io_bounding_box.Extent[1]);
	min_z = std::max(min_z, io_bounding_box.Center[2] - io_bounding_box.Extent[2]);

	io_bounding_box.Extent[0] = (max_x - min_x) / 2;
	io_bounding_box.Extent[1] = (max_y - min_y) / 2;
	io_bounding_box.Extent[2] = (max_z - min_z) / 2;
	io_bounding_box.Center[0] = (max_x + min_x) / 2;
	io_bounding_box.Center[1] = (max_y + min_y) / 2;
	io_bounding_box.Center[2] = (max_z + min_z) / 2;
	}

void KDTree::_CreateTree(std::vector<const Triangle3D*>& io_triangles, std::shared_ptr<KDTree::Node> io_node)
	{
	++recursion_depth;
	max_recursion_depth = std::max(recursion_depth, max_recursion_depth);
	_FitTBoxForTriangles(io_node->m_bounding_box, io_triangles);
	if (io_triangles.size() <= MAX_PRIMITIVES_PER_NODE)
		{
		for(size_t i = 0; i < io_triangles.size(); ++i)
			{
			io_node->m_triangles.push_back(io_triangles[i]);
			}
		io_node->m_triangles_in_subtree = io_node->m_triangles.size();
		--recursion_depth;
		return;
		}
		
	std::vector<const Triangle3D*> first_triangles;
	std::vector<const Triangle3D*> second_triangles;
	std::vector<const Triangle3D*> middle_triangles;
	
	auto divided_boxes = _DivideBoundingBox(io_node->m_bounding_box);	
	

	for(size_t i = 0; i < io_triangles.size(); ++i)
		{
		TTriangle3 triangle(io_triangles[i]->GetPoint(0), io_triangles[i]->GetPoint(1), io_triangles[i]->GetPoint(2));
		bool first_fits = IntersectionTest(triangle, divided_boxes.first);
		bool second_fits = IntersectionTest(triangle, divided_boxes.second);
		if (first_fits && second_fits)
			{
			middle_triangles.push_back(io_triangles[i]);
			}
		else if (first_fits)
			{
			first_triangles.push_back(io_triangles[i]);
			}
		else if (second_fits)
			{
			second_triangles.push_back(io_triangles[i]);
			}
		}
	std::shared_ptr<Node> middle_node;
	if (middle_triangles.size() > 0 && io_triangles.size() != first_triangles.size() && io_triangles.size() != second_triangles.size() && io_triangles.size() != middle_triangles.size())
		{
		middle_node.reset(new Node(io_node->m_bounding_box)); 
		}
	std::shared_ptr<Node> first_node;
	if (first_triangles.size() > 0 && io_triangles.size() != first_triangles.size() && io_triangles.size() != second_triangles.size() && io_triangles.size() != middle_triangles.size())
		{
		first_node.reset(new Node(divided_boxes.first)); 
		}
	std::shared_ptr<Node> second_node;
	if(second_triangles.size() > 0 && io_triangles.size() != second_triangles.size() && io_triangles.size() != first_triangles.size() && io_triangles.size() != middle_triangles.size())
		{
		second_node.reset(new Node(divided_boxes.second));
		}

	io_node->mp_left = first_node;
	io_node->mp_right = second_node;
	io_node->mp_middle = middle_node;
	if (first_node)
		{
		_CreateTree(first_triangles, first_node);
		io_node->m_triangles_in_subtree += first_node->m_triangles_in_subtree;
		}
	if (second_node)
		{
		_CreateTree(second_triangles, second_node);
		io_node->m_triangles_in_subtree += second_node->m_triangles_in_subtree;
		}
	if (middle_node)
		{
		_CreateTree(middle_triangles, middle_node);
		io_node->m_triangles_in_subtree += middle_node->m_triangles_in_subtree;
		}
	if (!first_node && !second_node && !middle_node)
		{
		for(size_t i = 0; i < io_triangles.size(); ++i)
			{
			io_node->m_triangles.push_back(io_triangles[i]);
			}
		io_node->m_triangles_in_subtree = io_node->m_triangles.size();
		--recursion_depth;
		return;
		}
	--recursion_depth;
	}

std::pair<TBox3, TBox3> KDTree::_DivideBoundingBox(const TBox3& i_box)
	{
	double max_dimention = std::numeric_limits<double>::min();
	int number_of_dimention = 0;
	for(int i = 0; i < 3; ++i)
		{
		if (max_dimention < i_box.Extent[i])
			{
			max_dimention = i_box.Extent[i];
			number_of_dimention = i;
			}
		}
	TBox3 first_box = i_box;
	first_box.Extent[number_of_dimention] /= 2;
	first_box.Center[number_of_dimention] += first_box.Extent[number_of_dimention];
	TBox3 second_box = i_box;
	second_box.Extent[number_of_dimention] /= 2;
	second_box.Center[number_of_dimention] -= second_box.Extent[number_of_dimention];
	return std::make_pair(first_box, second_box);
	}

void KDTree::_PickTriangle(std::set<const Triangle3D*>& io_triangles, const TLine3& io_line, const std::shared_ptr<KDTree::Node> i_node)
	{
	if (!i_node || !IntersectionTest(io_line, i_node->m_bounding_box))
		{
		return;
		}
	else
		{
		if(i_node->m_triangles.empty())
			{
			_PickTriangle(io_triangles, io_line, i_node->mp_left);
			_PickTriangle(io_triangles, io_line, i_node->mp_right);
			_PickTriangle(io_triangles, io_line, i_node->mp_middle);
			return;
			}
		else 
			{
			for(size_t i = 0; i < i_node->m_triangles.size(); ++i)
				{
				TTriangle3 triangle(i_node->m_triangles[i]->GetPoint(0), i_node->m_triangles[i]->GetPoint(1), i_node->m_triangles[i]->GetPoint(2));
				if (IntersectionTest(io_line, triangle))
					{
					io_triangles.insert(i_node->m_triangles[i]);
					}
				}
			return;
			}
		}
	}

std::vector<const Triangle3D*> KDTree::PickTriangles(const TLine3& i_line)
	{
	std::set<const Triangle3D*> result_set;
	_PickTriangle(result_set, i_line, mp_root);
	std::vector<const Triangle3D*> result;
	for(auto iter = result_set.begin(); iter != result_set.end(); ++iter)
		result.push_back(*iter);
	return result;
	}
